#include "cvutil.hpp"

#include <cmath>


namespace cv {


void init_timer(int64& initial)
{
  initial = getTickCount();
}

double seconds_spent(int64& initial)
{
  return (getTickCount() - initial) / getTickFrequency();
}


Point2f rect_center(Rect rect)
{
  Point tl = rect.tl();
  return Point2f(tl.x + rect.width/2., tl.y + rect.height/2.);
}

double dist(Point2f& a, Point2f& b) {
  return sqrt(dist2(a, b));
}

double dist2(Point2f& a, Point2f& b)
{
  return (b.x-a.x)*(b.x-a.x) + (b.y-a.y)*(b.y-a.y);
}

double line_angle(Point2f& orig, Point2f& dest) // degrees
{
  double dx = dest.x - orig.x;
  double dy = dest.y - orig.y;
  return atan2(dy, dx);
}

Mat square_mask(Size mask_size, int square_side, Point center)
{
  Mat mask = Mat::zeros(mask_size, CV_8UC1);

  mask.at<uchar>(center) = 255;

  Mat kernel = Mat::ones(square_side, square_side, CV_8UC1);
  dilate(mask, mask, kernel, Point(-1,-1), 1);
  
  return mask;
}


void findCornerSubPix(Mat& image, vector<Point2f>& corners,
                      Size window, Size zero_zone, CvTermCriteria criteria)
{
  int n_points = corners.size();

  IplImage _image = image;

  CvPoint2D32f* _corners = new CvPoint2D32f[n_points];
  for (int i = 0; i < n_points; ++i)
    _corners[i] = corners[i];

  CvSize _window = window;
  CvSize _zero_zone = zero_zone;

  cvFindCornerSubPix(&_image, _corners, n_points,
                     _window, _zero_zone, criteria);

  for (int i = 0; i < n_points; ++i)
    corners[i] = Point2f(_corners[i]);

  delete[] _corners;
}


}
